#include <rclcpp/rclcpp.hpp>                  // ROS 2 C++核心库，提供节点、发布订阅等基础功能
#include <livox_interfaces/msg/custom_msg.hpp> // Livox激光雷达自定义消息类型
#include <sensor_msgs/msg/point_cloud2.hpp>   // ROS标准点云消息类型
#include <pcl_conversions/pcl_conversions.h>  // PCL点云与ROS消息格式转换工具
#include <pcl/point_cloud.h>                  // PCL点云库核心类
#include <pcl/point_types.h>                  // PCL点类型定义（如XYZI点）
#include <pcl/filters/passthrough.h>          // PCL直通滤波器，用于按坐标范围过滤点
#include <pcl/filters/extract_indices.h>      // PCL索引提取器，用于按索引过滤点
#include <pcl/segmentation/extract_clusters.h> // PCL聚类提取算法
#include <pcl/kdtree/kdtree.h>                // PCL KD树，用于高效近邻搜索
#include <pcl/filters/voxel_grid.h>           // PCL体素网格滤波器，用于降采样
#include <cmath>                              // C++标准数学库
#include <thread>                             // C++多线程库
#include <mutex>                              // C++互斥锁，用于线程同步
#include <chrono>                             // C++时间库，用于计时
#include <deque>                              // C++双端队列，用于消息缓冲
#include <atomic>                             // C++原子变量，用于线程安全的标志位
#include <open3d/Open3D.h>                    // Open3D可视化库头文件

// 定义激光雷达处理器类，继承自ROS 2的Node类
class LidarProcessor : public rclcpp::Node
{
public:
    // 构造函数：初始化节点名称和成员变量
    LidarProcessor() : Node("lidar_processor"), 
                       is_running_(true),  // 线程运行标志初始化为true
                       frame_count_(0)     // 帧计数器初始化为0
    {
        // 声明ROS参数并设置默认值（这些参数可以从launch文件或命令行修改）
        this->declare_parameter("input_cloud", "/livox/lidar_avia");  // 输入点云话题
        this->declare_parameter("distance_threshold", 30.0);           // 最大检测距离（米）
        this->declare_parameter("voxel_size", 0.3);                    // 体素网格大小（米）
        this->declare_parameter("cluster_eps", 1.0);                   // 聚类距离阈值（米）
        this->declare_parameter("cluster_min_points", 10);             // 最小聚类点数
        this->declare_parameter("enable_clustering", false);           // 是否启用聚类
        this->declare_parameter("front_region_width", 5.0);            // 正前方区域宽度（米）
        this->declare_parameter("front_region_height", 5.0);           // 正前方区域高度（米）

        // 从ROS参数服务器获取参数值
        std::string input_topic = this->get_parameter("input_cloud").as_string();
        distance_threshold_ = this->get_parameter("distance_threshold").as_double();
        voxel_size_ = this->get_parameter("voxel_size").as_double();
        cluster_eps_ = this->get_parameter("cluster_eps").as_double();
        cluster_min_points_ = this->get_parameter("cluster_min_points").as_int();
        enable_clustering_ = this->get_parameter("enable_clustering").as_bool();
        
        // 计算正前方区域的半宽和半高（用于过滤判断）
        double front_width = this->get_parameter("front_region_width").as_double();
        double front_height = this->get_parameter("front_region_height").as_double();
        half_width_ = front_width / 2.0;    // Y方向半宽
        half_height_ = front_height / 2.0;  // Z方向半高

        exclude_front_ = true;  // 是否排除正前方区域（true=排除，false=保留）

        // 创建点云订阅者：订阅激光雷达数据
        // 参数说明：话题名、队列长度、回调函数
        sub_ = this->create_subscription<livox_interfaces::msg::CustomMsg>(
            input_topic, 50, std::bind(&LidarProcessor::callback, this, std::placeholders::_1));

        // 创建点云发布者：发布处理后的点云
        pcl_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/livox/pointcloud2", 10);

        // 打印初始化信息到控制台
        RCLCPP_INFO(this->get_logger(), "订阅: %s，发布可视化点云到: /livox/pointcloud2", input_topic.c_str());
        RCLCPP_INFO(this->get_logger(), "参数设置: 距离阈值=%.1f, 体素大小=%.2f, 正前方区域=%.1fx%.1f米, 聚类=%s",
                   distance_threshold_, voxel_size_, front_width, front_height, 
                   enable_clustering_ ? "开启" : "关闭");

        // 帧率计算参数初始化
        fps_window_size_ = 10;  // 计算最近10帧的平均帧率
        last_fps_print_time_ = this->now();  // 上次打印帧率的时间

        // 初始化一个默认点云（避免可视化窗口黑屏）
        dummy_cloud_ = std::make_shared<open3d::geometry::PointCloud>();
        dummy_cloud_->points_.push_back(Eigen::Vector3d(0, 0, 0));  // 添加原点
        dummy_cloud_->colors_.push_back(Eigen::Vector3d(1, 1, 1));  // 白色

        // 创建处理线程和可视化线程（分离数据处理和显示，提高效率）
        processing_thread_ = std::thread(&LidarProcessor::processing_loop, this);
        visualization_thread_ = std::thread(&LidarProcessor::visualization_loop, this);
    }

    // 析构函数：程序退出时清理资源
    ~LidarProcessor()
    {
        is_running_ = false;  // 通知线程停止运行
        
        // 等待线程结束（避免程序异常退出）
        if (processing_thread_.joinable()) {
            processing_thread_.join();
        }
        if (visualization_thread_.joinable()) {
            visualization_thread_.join();
        }
    }

private:
    // 回调函数：接收激光雷达数据（只负责接收，不做处理）
    void callback(const livox_interfaces::msg::CustomMsg::SharedPtr msg)
    {
        // 加锁保证线程安全（避免同时读写消息队列）
        std::lock_guard<std::mutex> lock(queue_mutex_);
        
        // 如果队列已满（超过10帧），丢弃最旧的数据
        if (msg_queue_.size() > 10) {
            msg_queue_.pop_front();  // 移除队首元素
            RCLCPP_WARN(this->get_logger(), "消息队列堆积，丢弃旧数据");
        }
        
        // 将新消息加入队列（存储拷贝，避免指针失效问题）
        msg_queue_.push_back(*msg);
    }

    // 处理线程主函数：负责点云的过滤、聚类等处理
    void processing_loop()
    {
        // 循环运行直到程序退出
        while (is_running_ && rclcpp::ok()) {
            // 如果消息队列为空，休眠10ms后再检查（避免空转占用CPU）
            if (msg_queue_.empty()) {
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                continue;
            }

            // 记录当前帧处理开始时间（用于计算帧率）
            auto frame_start_time = std::chrono::high_resolution_clock::now();
            livox_interfaces::msg::CustomMsg msg;  // 存储取出的消息
            
            // 从队列中取出消息（加锁保证线程安全）
            {
                std::lock_guard<std::mutex> lock(queue_mutex_);
                msg = msg_queue_.front();  // 获取队首消息
                msg_queue_.pop_front();    // 移除队首消息
            }

            // 将Livox消息格式转换为PCL点云格式（PCL库更适合点云处理）
            pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
            cloud->points.reserve(msg.points.size());  // 预分配内存，提高效率
            
            // 遍历消息中的每个点，转换为PCL的XYZI点（X/Y/Z坐标+反射强度）
            for (const auto& p : msg.points) {
                pcl::PointXYZI point;
                point.x = p.x;
                point.y = p.y;
                point.z = p.z;
                point.intensity = p.reflectivity;  // 反射强度
                cloud->points.push_back(point);
            }

            // 每10帧打印一次原始点云数量（用于调试）
            frame_count_++;
            if (frame_count_ % 10 == 0) {
                RCLCPP_INFO(this->get_logger(), "原始点云数量: %zu", cloud->points.size());
            }

            // 如果没有点，跳过后续处理
            if (cloud->points.empty()) {
                continue;
            }

            // 1. 距离过滤：保留X轴范围内的点（限制左右方向距离）
            pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZI>);
            pcl::PassThrough<pcl::PointXYZI> pass;  // 直通滤波器
            pass.setInputCloud(cloud);              // 设置输入点云
            pass.setFilterFieldName("x");           // 过滤X轴
            pass.setFilterLimits(-distance_threshold_, distance_threshold_);  // X范围
            pass.filter(*filtered_cloud);           // 执行过滤

            // 2. 高度过滤：保留Z轴范围内的点（限制上下方向距离）
            pass.setInputCloud(filtered_cloud);     // 使用上一步的过滤结果
            pass.setFilterFieldName("z");           // 过滤Z轴
            pass.setFilterLimits(-distance_threshold_/2, distance_threshold_/2);  // Z范围
            pass.filter(*filtered_cloud);           // 执行过滤

            // 3. 正前方区域过滤：排除（或保留）传感器正前方的矩形区域
            if (exclude_front_) {
                // 存储需要保留的点的索引
                pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
                inliers->indices.reserve(filtered_cloud->size()/2);  // 预分配内存
                
                // 遍历每个点，判断是否在需要排除的区域外
                for (size_t i = 0; i < filtered_cloud->points.size(); ++i) {
                    const auto& point = filtered_cloud->points[i];
                    
                    // 判断是否在正前方区域内：
                    // X>0（传感器正前方）、Y在[-2.5,2.5]（宽度5米）、Z在[-2.5,2.5]（高度5米）
                    bool in_front_region = (point.x > 0 && 
                                           std::abs(point.y) <= half_width_ && 
                                           std::abs(point.z) <= half_height_);
                    
                    // 如果不在正前方区域内，则保留该点（排除区域内的点）
                    if (!in_front_region) {
                        inliers->indices.push_back(i);
                    }
                }
                
                // 根据索引提取需要保留的点
                pcl::ExtractIndices<pcl::PointXYZI> extract;
                extract.setInputCloud(filtered_cloud);  // 输入点云
                extract.setIndices(inliers);            // 保留的点索引
                extract.filter(*filtered_cloud);        // 执行提取
            }

            // 4. 降采样：减少点云数量，提高处理速度
            pcl::PointCloud<pcl::PointXYZI>::Ptr downsampled_cloud(new pcl::PointCloud<pcl::PointXYZI>);
            if (filtered_cloud->points.size() > 100) {  // 如果点足够多才进行降采样
                pcl::VoxelGrid<pcl::PointXYZI> sor;      // 体素网格滤波器
                sor.setInputCloud(filtered_cloud);       // 输入点云
                sor.setLeafSize(voxel_size_, voxel_size_, voxel_size_);  // 体素大小
                sor.filter(*downsampled_cloud);          // 执行降采样
            } else {
                // 如果点太少，直接复制（不进行降采样）
                *downsampled_cloud = *filtered_cloud;
            }

            // 5. 聚类分析（可选功能，计算成本较高）
            if (enable_clustering_ && downsampled_cloud->points.size() > 100) {
                // 创建KD树用于近邻搜索（聚类算法需要）
                pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
                tree->setInputCloud(downsampled_cloud);

                // 存储聚类结果（每个聚类是一组点的索引）
                std::vector<pcl::PointIndices> cluster_indices;
                
                // 欧氏聚类提取器
                pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec;
                ec.setClusterTolerance(cluster_eps_);         // 聚类距离阈值（米）
                ec.setMinClusterSize(cluster_min_points_);    // 最小聚类点数
                ec.setMaxClusterSize(10000);                  // 最大聚类点数
                ec.setSearchMethod(tree);                     // 设置搜索方法（KD树）
                ec.setInputCloud(downsampled_cloud);          // 输入点云
                ec.extract(cluster_indices);                  // 执行聚类
            }

            // 发布处理后的点云（供其他ROS节点使用）
            sensor_msgs::msg::PointCloud2 pcl_msg;
            pcl::toROSMsg(*filtered_cloud, pcl_msg);  // 转换为ROS消息格式
            pcl_msg.header = msg.header;              // 保留原始消息的时间戳和坐标系
            pcl_pub_->publish(pcl_msg);               // 发布消息

            // 将处理后的点云转换为Open3D格式，供可视化线程使用
            {
                // 加锁保证线程安全（避免同时读写可视化数据）
                std::lock_guard<std::mutex> lock(visualization_mutex_);
                
                // 创建Open3D点云对象
                auto o3d_cloud = std::make_shared<open3d::geometry::PointCloud>();
                o3d_cloud->points_.resize(downsampled_cloud->size());
                o3d_cloud->colors_.reserve(downsampled_cloud->size());
                
                // 反射强度范围（用于颜色映射）
                double min_intensity = 0.0, max_intensity = 255.0;
                
                // 遍历每个点，设置坐标和颜色
                for (size_t i = 0; i < downsampled_cloud->size(); ++i) {
                    const auto& p = downsampled_cloud->points[i];
                    // 设置点坐标
                    o3d_cloud->points_[i] = Eigen::Vector3d(p.x, p.y, p.z);
                    
                    // 根据反射强度设置颜色（低反射率→蓝色，高反射率→红色）
                    double norm_intensity = std::min(1.0, std::max(0.0, 
                                           (p.intensity - min_intensity) / (max_intensity - min_intensity)));
                    o3d_cloud->colors_.push_back(Eigen::Vector3d(norm_intensity, 0.5*(1-norm_intensity), 1-norm_intensity));
                }
                
                // 更新可视化点云，并标记有新数据
                visualization_cloud_ = o3d_cloud;
                new_cloud_available_ = true;
            }

            // 计算并打印帧率
            calculate_fps(frame_start_time);
        }
    }

    // 可视化线程主函数：负责点云的显示
    void visualization_loop()
    {
        // 创建Open3D可视化窗口
        open3d::visualization::Visualizer vis;
        if (!vis.CreateVisualizerWindow("LiDAR Point Cloud Visualization", 1024, 768)) {
            RCLCPP_ERROR(this->get_logger(), "无法创建可视化窗口！");
            return;
        }
        
        // 添加初始点云（避免窗口黑屏）
        vis.AddGeometry(dummy_cloud_);
        
        // 优化渲染设置
        auto& render_option = vis.GetRenderOption();
        render_option.background_color_ = Eigen::Vector3d(0, 0, 0);  // 黑色背景（点更清晰）
        render_option.point_size_ = 2.0;  // 点的大小
        
        bool first_update = true;  // 是否是第一次更新点云
        std::shared_ptr<open3d::geometry::PointCloud> current_cloud = dummy_cloud_;
        
        // 循环运行直到程序退出
        while (is_running_ && rclcpp::ok()) {
            // 检查是否有新的点云数据
            {
                std::lock_guard<std::mutex> lock(visualization_mutex_);
                if (new_cloud_available_) {
                    // 移除旧点云
                    vis.RemoveGeometry(current_cloud);
                    
                    // 更新为新点云
                    current_cloud = visualization_cloud_;
                    vis.AddGeometry(current_cloud);
                    
                    // 第一次更新时调整视角（方便观察）
                    if (first_update) {
                        auto& view_control = vis.GetViewControl();
                        view_control.SetFront(Eigen::Vector3d(0, -1, 0));  // 相机朝向
                        view_control.SetLookat(Eigen::Vector3d(0, 0, -1)); // 观察点
                        view_control.SetUp(Eigen::Vector3d(0, 0, 1));      // 上方向
                        view_control.SetZoom(0.3);                         // 缩放比例
                        first_update = false;
                    }
                    
                    new_cloud_available_ = false;  // 重置新数据标志
                    
                    // 打印当前可视化的点云数量（用于调试）
                    RCLCPP_INFO(this->get_logger(), "可视化点云数量: %zu", current_cloud->points_.size());
                }
            }
            
            // 刷新窗口事件和渲染
            vis.PollEvents();
            vis.UpdateRender();
            
            // 控制可视化帧率（约30 FPS），避免占用过多CPU
            std::this_thread::sleep_for(std::chrono::milliseconds(33));
        }
        
        // 关闭可视化窗口
        vis.DestroyVisualizerWindow();
    }

    // 计算帧率和处理时间
    void calculate_fps(const std::chrono::high_resolution_clock::time_point& frame_start)
    {
        // 计算当前帧的处理时间（毫秒）
        auto frame_end = std::chrono::high_resolution_clock::now();
        double frame_time_ms = std::chrono::duration<double, std::milli>(frame_end - frame_start).count();
        
        // 保存最近的帧处理时间（加锁保证线程安全）
        {
            std::lock_guard<std::mutex> lock(fps_mutex_);
            frame_times_.push_back(frame_time_ms);
            // 如果超过窗口大小，移除最旧的记录
            if (frame_times_.size() > fps_window_size_) {
                frame_times_.pop_front();
            }
        }
        
        // 每1秒打印一次帧率统计
        auto now = this->now();
        auto elapsed = now - last_fps_print_time_;
        if (elapsed.seconds() >= 1.0) {
            std::lock_guard<std::mutex> lock(fps_mutex_);
            
            // 计算总处理时间
            double total_time = 0;
            for (double t : frame_times_) {
                total_time += t;
            }
            
            // 计算平均帧率
            double avg_fps = 0;
            if (total_time > 0) {
                avg_fps = frame_times_.size() / (total_time / 1000.0);  // 转换为秒
            }
            
            // 打印统计信息
            RCLCPP_INFO(this->get_logger(), "平均帧率: %.1f FPS, 平均处理时间: %.1f ms", 
                       avg_fps, total_time / frame_times_.size());
            
            // 更新上次打印时间
            last_fps_print_time_ = now;
        }
    }

    // ROS相关成员变量
    rclcpp::Subscription<livox_interfaces::msg::CustomMsg>::SharedPtr sub_;  // 点云订阅者
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pcl_pub_;     // 点云发布者

    // 数据处理参数
    double distance_threshold_;    // 最大检测距离
    bool exclude_front_;           // 是否排除正前方区域
    double half_width_;            // 正前方区域半宽（Y方向）
    double half_height_;           // 正前方区域半高（Z方向）
    double cluster_eps_;           // 聚类距离阈值
    int cluster_min_points_;       // 最小聚类点数
    bool enable_clustering_;       // 是否启用聚类
    double voxel_size_;            // 体素大小

    // 多线程相关成员
    std::thread processing_thread_;    // 点云处理线程
    std::thread visualization_thread_; // 可视化线程
    std::atomic<bool> is_running_;     // 线程运行标志（原子变量确保线程安全）
    size_t frame_count_;               // 帧计数器

    // 消息队列：用于接收线程和处理线程之间的通信
    std::deque<livox_interfaces::msg::CustomMsg> msg_queue_;  // 消息队列
    std::mutex queue_mutex_;                                  // 队列互斥锁

    // 可视化相关成员
    std::mutex visualization_mutex_;                          // 可视化数据互斥锁
    std::shared_ptr<open3d::geometry::PointCloud> visualization_cloud_;  // 待可视化的点云
    std::shared_ptr<open3d::geometry::PointCloud> dummy_cloud_;          // 初始点云
    std::atomic<bool> new_cloud_available_;                    // 新点云可用标志

    // 帧率计算相关成员
    std::mutex fps_mutex_;                          // 帧率数据互斥锁
    std::deque<double> frame_times_;                // 存储最近的帧处理时间（毫秒）
    size_t fps_window_size_;                        // 帧率计算窗口大小
    rclcpp::Time last_fps_print_time_;              // 上次打印帧率的时间
};

// 主函数：程序入口
int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);  // 初始化ROS 2
    auto node = std::make_shared<LidarProcessor>();  // 创建激光雷达处理器节点
    rclcpp::spin(node);        // 运行节点（进入事件循环）
    rclcpp::shutdown();        // 关闭ROS2
    return 0;
}
